#ifndef CSERVOWINDOW_H_
#define CSERVOWINDOW_H_

#include <buola/gui/containers/cmosaiccontainer.h>

#include <buola/scene.h>
#include <buola/scene/csceneview.h>
#include <buola/scene/geodes/cpointcloud.h>
#include <buola/widgets/cimagebox.h>
#include <buola/gui/containers/clistcontainer.h>
#include <buola/widgets/cnumbox.h>
#include <buola/widgets/ccheckbox.h>
#include <buola/cv/cvisualservoing.h>
#include <buola/io/csocketacceptor.h>

#include <robot/kuka/cvirtualarm.h>
#include <robot/sdh/cvirtualhand.h>

namespace buola { namespace simulator {

class CServoWindow : public gui::CMosaicContainer
{
public:
    CServoWindow(const io::CURI &pURDF);
    CServoWindow(const io::CURI &pURDF,const io::CURI &pPCD,const io::CURI &pJoints);
    CServoWindow(const io::CURI &pURDF,const io::CURI &pRGB,const io::CURI &pDepth,const io::CURI &pJoints);
    virtual ~CServoWindow();

private:
    void SetupWindow();
    void SetupScene(const io::CURI &pURDF,const io::CURI &pJoints);
    void SetupScene(const io::CURI &pURDF);
    void SetupJoints(const io::CURI &pJoints);
    void SetupJoints(const std::array<double,45> &pJoints);
    void SetupViews();
    void SetupInput();
    void SetupInput(const float *pPos,const uint8_t *pRGB);
    void SetupInput(const io::CURI &pPCD);
    void SetupInput(const io::CURI &pRGB,const io::CURI &pDepth);

    void ResetServo();
    void ShakeServo();
    void SaveMask();
    void NextIteration(bool pReally=true,int pCount=1);

    void OnCoordsBox(int i);
    void OnCommitHook(const CRegion &pRegion);
    
    void OnNewConnection(io::PSocket pSocket);

private:
    gui::PToolBar mToolBar;
    
    img::CImage_xyz32f mTargetXYZ;
    img::CImage_gray32f mTargetDepth;
    img::CImage_rgb8 mTargetImage;
    img::CImage_bgra8 mDirectionsImage;
    img::CImage_rgb8 mInputImage;
    img::CImage_gray8 mTransformIndex;
    
    cvi::CVisualServoing mVVS;
    std::string mFrame;
    
    scene::PScene mScene,mCloudScene,mObjectScene;
    scene::PMatTransform mCloudTransform,mObjectTransform;

    scene::PTransform mOpticalFrame;
    
    scene::PPerspectiveCamera mVVSCamera,mSceneCamera,mObjectCamera;

    scene::PSceneView mSceneView;
    scene::PPointCloud mSceneCloud;
    scene::PSceneView mObjectView;
    scene::PPointCloud mObjectCloud;
    gui::PImageBox mImageBox[3];
    
    gui::PListContainer mNumbersBox;
    gui::PNumBox mCoordsBox[6];
    gui::PCheckBox m2DBox;
    gui::PCheckBox m3DBox;

    mat::CMat34d mKinectTrans;

    std::unique_ptr<io::CSocketAcceptor> mAcceptor;
};

/*namespace simulator*/ } /*namespace buola*/ }

#endif /* CSERVOWINDOW_H_ */
